import ctypes
import time
import sys
import os

# 添加zhongling目录到Python路径，以便导入Motor_ctl.py
# 使用绝对路径确保正确导入
zhongling_path = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'zhongling'))
if zhongling_path not in sys.path:
    sys.path.insert(0, zhongling_path)

from Motor_ctl import (
    CANMessageSequence,  # 复用CAN收发核心类
    init_can_device,     # 复用CAN设备初始化函数
    close_can_device,    # 复用CAN设备关闭函数
    TYPE_CANFD,            # CAN帧类型常量
    STATUS_OK            # 状态码常量
)

# -------------------------- 协议常量定义（严格遵循文档6.3节） --------------------------
# 功能码（文档6.3.1-6.3.5节）
FUNC_MOTOR_ENABLE = 0xF3       # 电机使能控制
FUNC_SPEED_MODE = 0xF6          # 速度模式控制
FUNC_POSITION_MODE = 0xFD       # 位置模式控制
FUNC_IMMEDIATE_STOP = 0xFE     # 立即停止
FUNC_SYNC_MOTION = 0xFF        # 多机同步运动
FUNC_SET_ZERO_POS = 0x93       # 设置单圈回零零点
FUNC_TRIGGER_HOMING = 0x9A     # 触发回零
FUNC_ABORT_HOMING = 0x9C       # 中断回零
FUNC_READ_HOMING_PARAM = 0x22  # 读取回零参数
FUNC_MODIFY_HOMING_PARAM = 0x4C# 修改回零参数
FUNC_CALIBRATE_ENCODER = 0x06  # 触发编码器校准
FUNC_CLEAR_POS = 0x0A          # 位置清零
FUNC_CLEAR_LOCK = 0x0E         # 解除堵转保护
FUNC_RESTORE_FACTORY = 0x0F    # 恢复出厂设置
FUNC_READ_VERSION = 0x1F       # 读取固件/硬件版本
FUNC_READ_RL = 0x20            # 读取相电阻/电感
FUNC_READ_POS_PID = 0x21       # 读取位置环PID
FUNC_READ_BUS_VOLT = 0x24      # 读取总线电压
FUNC_READ_PHASE_CUR = 0x27     # 读取相电流
FUNC_READ_ENCODER = 0x31       # 读取校准后编码器值
FUNC_READ_PULSE = 0x32         # 读取输入脉冲数
FUNC_READ_TARGET_POS = 0x33    # 读取目标位置
FUNC_READ_REAL_SPEED = 0x35    # 读取实时转速
FUNC_READ_REAL_POS = 0x36      # 读取实时位置
FUNC_READ_POS_ERROR = 0x37     # 读取位置误差
FUNC_READ_MOTOR_STATE = 0x3A   # 读取电机状态标志
FUNC_READ_DRIVE_PARAM = 0x42    # 读取驱动配置参数
FUNC_READ_SYSTEM_STATE = 0x43  # 读取系统状态参数
FUNC_MODIFY_MICROSTEP = 0x84   # 修改细分
FUNC_MODIFY_ID = 0xAE          # 修改ID地址
FUNC_SWITCH_OPEN_CLOSED = 0x46 # 切换开环/闭环
FUNC_MODIFY_OPEN_CUR = 0x44    # 修改开环电流
FUNC_MODIFY_DRIVE_PARAM = 0x48 # 修改驱动配置参数
FUNC_MODIFY_POS_PID = 0x4A     # 修改位置环PID
FUNC_SAVE_SPEED_PARAM = 0xF7    # 存储速度参数（上电自动运行）
FUNC_MODIFY_SPEED_SCALE = 0x4F  # 修改速度缩小10倍

# 回零模式（文档6.3.2节）
HOMING_MODE_NEAREST = 0x00     # 单圈就近回零
HOMING_MODE_DIR = 0x01         # 单圈方向回零
HOMING_MODE_SENSELESS = 0x02   # 多圈无限位碰撞回零
HOMING_MODE_ENDSTOP = 0x03     # 多圈有限位开关回零

# 位置模式（文档6.3.1节）
POS_MODE_RELATIVE = 0x00       # 相对位置模式
POS_MODE_ABSOLUTE = 0x01       # 绝对位置模式

# 校验方式（文档6.2.1节，默认0x6B）
CHECK_SUM = 0x6B

# -------------------------- Emm42_V5.0 CAN控制类（继承核心收发类） --------------------------
class Emm42_CAN_Controller(CANMessageSequence):
    def __init__(self, channel_handle, device_id=1, response_timeout=5):
        """
        初始化Emm42驱动器CAN控制器
        :param channel_handle: CAN通道句柄（由init_can_device返回）
        :param device_id: 驱动器ID地址（1-255，默认1）
        :param response_timeout: 响应超时时间（秒）
        """
        # 父类初始化（send_id=驱动器ID左移8位，response_id=同send_id）
        send_id = (device_id << 8) & 0xFFFFFF00  # CAN帧ID：ID左移8位，低8位为分包序号
        super().__init__(
            channel_handle=channel_handle,
            send_id=send_id,
            response_id=send_id,  # 响应ID与发送ID一致（文档6.4节）
            response_timeout=response_timeout
        )
        self.device_id = device_id  # 驱动器ID
        self.current_send_id = send_id  # 当前发送帧ID（含分包序号）

    # -------------------------- 1. 基础控制功能（文档6.3.1节） --------------------------
    def motor_enable(self, enable=True, sync_flag=0x00):
        """
        电机使能/失能控制（文档6.3.1节）
        :param enable: True=使能，False=失能
        :param sync_flag: 多机同步标志（0=不启用，1=启用）
        :return: bool：操作是否成功
        """
        enable_state = 0x01 if enable else 0x00
        # 命令格式：地址 + 0xF3 + 0xAB + 使能状态 + 同步标志 + 校验
        cmd = [
            FUNC_MOTOR_ENABLE, 0xAB,
            enable_state, sync_flag, CHECK_SUM
        ]
        # 发送命令（单包<8字节，分包序号00，send_id低8位设0）
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(
            send_id=self.current_send_id,
            data_list=cmd,
            is_ext_frame=True  # 强制扩展帧（文档6.4节）
        )
        if not send_ok:
            print(f"❌ 电机{enable_state}使能命令发送失败")
            return False
        
        # 接收响应（响应格式：功能码 + 状态码 + 校验）
        resp_ok, resp_data = self._receive_can_data(
            target_id=self.current_send_id,
            is_ext_frame=True
        )
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ 电机{enable_state}使能响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_MOTOR_ENABLE:
            print(f"❌ 电机使能响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        # 状态码：0x02=成功，0xE2=条件不满足，0xEE=命令错误
        status = resp_data[1]
        if status == 0x02:
            print(f"✅ 电机{'使能' if enable else '失能'}成功")
            return True
        else:
            print(f"❌ 电机{'使能' if enable else '失能'}失败，状态码:0x{status:02X}")
            return False

    def speed_control(self, direction, speed_rpm, accel_level=0x00, sync_flag=0x00):
        """
        速度模式控制（文档6.3.1节）
        :param direction: 方向（0x00=CW顺时针，0x01=CCW逆时针）
        :param speed_rpm: 转速（0.1-3000RPM，需转16进制，如1500=0x05DC）
        :param accel_level: 加速度档位（0-255，0=无加减速）
        :param sync_flag: 多机同步标志（0=不启用，1=启用）
        :return: bool：操作是否成功
        """
        # 转速转2字节（低字节在前）
        speed_low = speed_rpm & 0xFF
        speed_high = (speed_rpm >> 8) & 0xFF
        # 命令格式：地址 + 0xF6 + 方向 + 速度(2B) + 加速度 + 同步标志 + 校验
        cmd = [
            FUNC_SPEED_MODE, direction,
            speed_high, speed_low, accel_level,
            sync_flag, CHECK_SUM
        ]
        # 发送命令（单包8字节，分包序号00）
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(
            send_id=self.current_send_id,
            data_list=cmd,
            is_ext_frame=True
        )
        if not send_ok:
            print(f"❌ 速度控制命令发送失败（目标转速：{speed_rpm}RPM）")
            return False
        
        # 接收响应
        resp_ok, resp_data = self._receive_can_data(
            target_id=self.current_send_id,
            is_ext_frame=True
        )
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ 速度控制响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_SPEED_MODE:
            print(f"❌ 速度控制响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        status = resp_data[1]
        if status == 0x02:
            print(f"✅ 速度控制成功（方向:0x{direction:02X}，转速:{speed_rpm}RPM）")
            return True
        else:
            print(f"❌ 速度控制失败，状态码:0x{status:02X}（检查使能/堵转）")
            return False

    def position_control(self, direction, speed_rpm, accel_level, pulse_num, pos_mode, sync_flag=0x00):
        """
        位置模式控制（文档6.3.1节，命令12字节，需拆2包发送）
        :param direction: 方向（0x00=CW，0x01=CCW）
        :param speed_rpm: 转速（0.1-3000RPM）
        :param accel_level: 加速度档位（0-255）
        :param pulse_num: 脉冲数（0-65535，16细分下3200脉冲=1圈）
        :param pos_mode: 位置模式（POS_MODE_RELATIVE/POS_MODE_ABSOLUTE）
        :param sync_flag: 多机同步标志（0=不启用，1=启用）
        :return: bool：操作是否成功
        """
        # 参数转多字节（低字节在前）
        speed_low = speed_rpm & 0xFF
        speed_high = (speed_rpm >> 8) & 0xFF
        pulse_1 = pulse_num & 0xFF
        pulse_2 = (pulse_num >> 8) & 0xFF
        pulse_3 = (pulse_num >> 16) & 0xFF
        pulse_4 = (pulse_num >> 24) & 0xFF

        # 拆包1（第0包，8字节）：地址 + 0xFD + 方向 + 速度(2B) + 加速度 + 脉冲(2B)
        cmd_pack0 = [
            FUNC_POSITION_MODE, direction,
            speed_high, speed_low, accel_level,
            pulse_4, pulse_3, pulse_2
        ]
        # 拆包2（第1包，6字节）：脉冲(2B) + 位置模式 + 同步标志 + 校验
        cmd_pack1 = [
            0xfd,pulse_1, pos_mode,
            sync_flag, CHECK_SUM
        ]

        # 发送第0包（分包序号00）
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok0 = self._send_can_data(
            send_id=self.current_send_id,
            data_list=cmd_pack0,
            is_ext_frame=True
        )
        if not send_ok0:
            print(f"❌ 位置控制第0包发送失败")
            return False
        
        # 发送第1包（分包序号01）
        self.current_send_id = (self.device_id << 8) | 0x01
        send_ok1 = self._send_can_data(
            send_id=self.current_send_id,
            data_list=cmd_pack1,
            is_ext_frame=True
        )
        if not send_ok1:
            print(f"❌ 位置控制第1包发送失败")
            return False
        
        # 接收响应（两包共用一个响应）
        resp_ok, resp_data = self._receive_can_data(
            target_id=(self.device_id << 8) | 0x00,
            is_ext_frame=True
        )
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ 位置控制响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_POSITION_MODE:
            print(f"❌ 位置控制响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        status = resp_data[1]
        if status == 0x02:
            mode_name = "相对位置" if pos_mode == POS_MODE_RELATIVE else "绝对位置"
            print(f"✅ 位置控制成功（{mode_name}，脉冲数:{pulse_num}，转速:{speed_rpm}RPM）")
            return True
        else:
            print(f"❌ 位置控制失败，状态码:0x{status:02X}")
            return False

    def immediate_stop(self, sync_flag=0x00):
        """立即停止（文档6.3.1节）"""
        cmd = [FUNC_IMMEDIATE_STOP, 0x98, sync_flag, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 立即停止命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ 立即停止响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_IMMEDIATE_STOP:
            print(f"❌ 立即停止响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        if resp_data[1] == 0x02:
            print(f"✅ 电机已立即停止")
            return True
        else:
            print(f"❌ 立即停止操作失败，状态码:0x{resp_data[1]:02X}")
            return False

    def sync_motion(self):
        """多机同步运动（文档6.3.1节）"""
        cmd = [FUNC_SYNC_MOTION, 0x66, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 同步运动命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ 同步运动响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_SYNC_MOTION:
            print(f"❌ 同步运动响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        if resp_data[1] == 0x02:
            print(f"✅ 多机同步运动触发成功")
            return True
        else:
            print(f"❌ 同步运动触发失败，状态码:0x{resp_data[1]:02X}")
            return False

    # -------------------------- 2. 原点回零功能（文档6.3.2、7章） --------------------------
    def set_homing_zero(self, save_flag=0x01):
        """设置单圈回零零点（文档6.3.2节）"""
        cmd = [FUNC_SET_ZERO_POS, 0x88, save_flag, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 回零零点设置命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ 回零零点设置响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_SET_ZERO_POS:
            print(f"❌ 回零零点设置响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        if resp_data[1] == 0x02:
            print(f"✅ 单圈回零零点设置成功（{'断电保存' if save_flag==0x01 else '临时生效'}）")
            return True
        else:
            print(f"❌ 回零零点设置失败，状态码:0x{resp_data[1]:02X}（需先校准编码器）")
            return False

    def trigger_homing(self, homing_mode, sync_flag=0x00):
        """触发回零（文档6.3.2、7章）"""
        if homing_mode not in [0x00, 0x01, 0x02, 0x03]:
            print(f"❌ 无效回零模式（需为00-03）")
            return False
        
        mode_names = ["单圈就近回零", "单圈方向回零", "多圈无限位回零", "多圈有限位开关回零"]
        cmd = [FUNC_TRIGGER_HOMING, homing_mode, sync_flag, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ {mode_names[homing_mode]}命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ {mode_names[homing_mode]}响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_TRIGGER_HOMING:
            print(f"❌ {mode_names[homing_mode]}响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        if resp_data[1] == 0x02:
            print(f"✅ {mode_names[homing_mode]}触发成功")
            return True
        else:
            print(f"❌ {mode_names[homing_mode]}触发失败，状态码:0x{resp_data[1]:02X}（检查零点/使能）")
            return False

    def abort_homing(self):
        """中断回零（文档6.3.2节）"""
        cmd = [FUNC_ABORT_HOMING, 0x48, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 中断回零命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 3:
            print(f"❌ 中断回零响应异常")
            return False
        
        # 校验功能码
        if resp_data[0] != FUNC_ABORT_HOMING:
            print(f"❌ 中断回零响应功能码错误:0x{resp_data[0]:02X}")
            return False
        
        if resp_data[1] == 0x02:
            print(f"✅ 回零操作已中断")
            return True
        else:
            print(f"❌ 中断回零失败，状态码:0x{resp_data[1]:02X}（当前无回零操作）")
            return False

    def read_homing_param(self):
        """读取回零参数（功能码0x22，子索引0x6b）"""
        # 定义需要接收的帧ID列表
        frame_ids = [0x100, 0x101, 0x102]
        frame_data = {}  # 存储接收到的帧数据
        
        # 为每个帧ID分别发送命令并接收数据
        for frame_id in frame_ids:
            # 发送读取回零参数命令（使用原来的发送方式）
            cmd = [FUNC_READ_HOMING_PARAM, CHECK_SUM]
            self.current_send_id = (self.device_id << 8) | 0x00
            send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
            if not send_ok:
                print(f"❌ 发送读取回零参数命令到0x{frame_id:x}失败")
                return False, {}
            
            print(f"\n📥 等待接收回零参数数据（帧ID: 0x{frame_id:x}）...")
            
            # 接收该帧数据
            resp_ok, resp_data = self._receive_can_data(
                target_id=frame_id,
                is_ext_frame=True,
                timeout=2
            )
            
            if not resp_ok:
                print(f"❌ 接收帧0x{frame_id:x}数据超时")
                return False, {}
            
            if len(resp_data) < 3:
                print(f"❌ 帧0x{frame_id:x}数据长度不足")
                return False, {}
            
            # 校验功能码和状态码
            if resp_data[0] != FUNC_READ_HOMING_PARAM:  # 功能码应为FUNC_READ_HOMING_PARAM
                print(f"❌ 帧0x{frame_id:x}功能码错误，期望0x{FUNC_READ_HOMING_PARAM:02x}，实际0x{resp_data[0]:02x}")
                return False, {}
            
            # 存储有效数据（跳过功能码和状态码）
            frame_data[frame_id] = resp_data[1:]
            print(f"✅ 成功接收帧0x{frame_id:x}数据，长度: {len(resp_data[1:])}字节")
            print(f"   数据内容: {[hex(d) for d in resp_data[1:]]}")
        
        # 检查是否收到了所有需要的帧
        if len(frame_data) < len(frame_ids):
            missing_ids = [f"0x{id:x}" for id in frame_ids if id not in frame_data]
            print(f"❌ 读取回零参数数据不完整，缺失帧ID: {', '.join(missing_ids)}")
            return False, {}
        
        # 按ID顺序合并数据
        all_resp_data = []
        for frame_id in frame_ids:
            all_resp_data.extend(frame_data[frame_id])
        
        print(f"✅ 成功接收并合并 {len(frame_ids)} 帧数据，总长度: {len(all_resp_data)} 字节")
        print(f"📋 合并后的完整数据（十六进制）: {[hex(x) for x in all_resp_data]}")
        
        # 检查数据长度是否足够（16位有效字节位：跳过功能码0x22和校验位0x6b）
        if len(all_resp_data) < 16:
            print(f"❌ 回零参数数据长度不足，期望16字节有效数据，实际{len(all_resp_data)}字节")
            return False, {}
        
        # 解析回零参数
        try:
            # 根据实际数据格式重新解析
            # 数据格式：[0x0, 0x0, 0x0, 0x1e, 0x0, 0x0, 0x27, 0x10, 0x1, 0x2c, 0x3, 0x20, 0x0, 0x3c, 0x0, 0x6b]
            # 正确的解析应该是：
            # 第0字节：回零模式（0x00=正向限位，0x01=负向限位，0x02=原点开关）
            # 第1字节：回零方向（0x00=正向，0x01=负向）
            # 第2-3字节：回零转速（16位整数，单位rpm）
            # 第4-7字节：回零超时时间（32位整数，单位ms）
            # 第8-9字节：无限位碰撞回零检测转速（16位整数，单位rpm）
            # 第10-11字节：无限位碰撞回零检测电流（16位整数，单位mA）
            # 第12-13字节：无限位碰撞回零检测时间（16位整数，单位ms）
            # 第14字节：是否使能上电自动触发回零功能（0x00=不使能，0x01=使能）
            # 第15字节：校验位（0x6b）
            
            print(f"🔍 调试信息 - 原始数据: {all_resp_data}")
            
            # 第0字节：回零模式
            homing_method = all_resp_data[0]
            print(f"🔍 回零模式: all_resp_data[0] = {all_resp_data[0]}")
            
            # 第1字节：回零方向
            homing_direction = all_resp_data[1]
            print(f"🔍 回零方向: all_resp_data[1] = {all_resp_data[1]}")
            
            # 第2-3字节：回零转速（16位整数，单位rpm）
            # 注意：小端字节序，低位在前，高位在后
            homing_speed = (all_resp_data[2] << 8) | all_resp_data[3]
            print(f"🔍 回零转速: all_resp_data[2] = {all_resp_data[2]}, all_resp_data[3] = {all_resp_data[3]}, 结果 = {homing_speed}")
            
            # 第4-7字节：回零超时时间（32位整数，单位ms）
            # 大端字节序：all_resp_data[4]是最高位，all_resp_data[7]是最低位
            homing_timeout = (all_resp_data[4] << 24) | (all_resp_data[5] << 16) | (all_resp_data[6] << 8) | all_resp_data[7]
            print(f"🔍 回零超时时间: all_resp_data[4] = {all_resp_data[4]}, all_resp_data[5] = {all_resp_data[5]}, all_resp_data[6] = {all_resp_data[6]}, all_resp_data[7] = {all_resp_data[7]}, 结果 = {homing_timeout}")
            
            # 第8-9字节：无限位碰撞回零检测转速（16位整数，单位rpm）
            collision_homing_speed = (all_resp_data[8] << 8) | all_resp_data[9]
            print(f"🔍 碰撞检测转速: all_resp_data[8] = {all_resp_data[8]}, all_resp_data[9] = {all_resp_data[9]}, 结果 = {collision_homing_speed}")
            
            # 第10-11字节：无限位碰撞回零检测电流（16位整数，单位mA）
            collision_homing_current = (all_resp_data[10] << 8) | all_resp_data[11]
            print(f"🔍 碰撞检测电流: all_resp_data[10] = {all_resp_data[10]}, all_resp_data[11] = {all_resp_data[11]}, 结果 = {collision_homing_current}")
            
            # 第12-13字节：无限位碰撞回零检测时间（16位整数，单位ms）
            collision_homing_time = (all_resp_data[12] << 8) | all_resp_data[13]
            print(f"🔍 碰撞检测时间: all_resp_data[12] = {all_resp_data[12]}, all_resp_data[13] = {all_resp_data[13]}, 结果 = {collision_homing_time}")
            
            # 第14字节：是否使能上电自动触发回零功能
            auto_homing_enable = all_resp_data[14]
            print(f"🔍 自动回零使能: all_resp_data[14] = {all_resp_data[14]}")
            
            homing_params = {
                'homing_method': homing_method,
                'homing_direction': homing_direction,
                'homing_speed': homing_speed,  # 单位rpm
                'homing_timeout': homing_timeout,    # 单位ms
                'collision_homing_speed': collision_homing_speed,  # 单位rpm
                'collision_homing_current': collision_homing_current,  # 单位mA
                'collision_homing_time': collision_homing_time,    # 单位ms
                'auto_homing_enable': auto_homing_enable
            }
            
            # 打印解析结果
            method_map = {0: '正向限位', 1: '负向限位', 2: '原点开关'}
            direction_map = {0: '正向', 1: '负向'}
            enable_map = {0: '不使能', 1: '使能'}
            
            print(f"\n📊 回零参数解析结果：")
            print(f"   回零模式: {homing_method} ({method_map.get(homing_method, '未知')})")
            print(f"   回零方向: {homing_direction} ({direction_map.get(homing_direction, '未知')})")
            print(f"   回零转速: {homing_params['homing_speed']} rpm")
            print(f"   回零超时时间: {homing_params['homing_timeout']} ms")
            print(f"   无限位碰撞检测转速: {homing_params['collision_homing_speed']} rpm")
            print(f"   无限位碰撞检测电流: {homing_params['collision_homing_current']} mA")
            print(f"   无限位碰撞检测时间: {homing_params['collision_homing_time']} ms")
            print(f"   上电自动回零: {homing_params['auto_homing_enable']} ({enable_map.get(homing_params['auto_homing_enable'], '未知')})")
            
            return True, homing_params
            
        except Exception as e:
            print(f"❌ 解析回零参数失败: {e}")
            return False, {}

    # -------------------------- 3. 触发动作功能（文档6.3.3节） --------------------------
    def calibrate_encoder(self):
        """触发编码器校准（文档6.3.3、3.4节）"""
        cmd = [FUNC_CALIBRATE_ENCODER, 0x45, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 编码器校准命令发送失败")
            return False
        
        print(f"⏳ 编码器校准中（电机将正转1圈→反转1圈）...")
        time.sleep(10)  # 校准需约10秒
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if resp_ok and resp_data[2] == 0x02:
            print(f"✅ 编码器校准成功（已测量相电阻/电感）")
            return True
        else:
            print(f"❌ 编码器校准失败（检查磁铁安装/开环模式）")
            return False

    def clear_position(self):
        """位置清零（文档6.3.3节）"""
        cmd = [FUNC_CLEAR_POS, 0x6D, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 位置清零命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if resp_ok and resp_data[1] == 0x02:
            print(f"✅ 电机位置/误差/脉冲数已清零")
            return True
        else:
            print(f"❌ 位置清零失败")
            return False

    def clear_lock_protection(self):
        """解除堵转保护（文档6.3.3节）"""
        cmd = [FUNC_CLEAR_LOCK, 0x52, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 解除堵转保护命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if resp_ok and resp_data[2] == 0x02:
            print(f"✅ 堵转保护已解除（无需重新上电）")
            return True
        else:
            print(f"❌ 解除堵转保护失败（当前无堵转保护）")
            return False

    def restore_factory(self):
        """恢复出厂设置（文档6.3.3节）"""
        cmd = [FUNC_RESTORE_FACTORY, 0x5F, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 恢复出厂命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if resp_ok and resp_data[2] == 0x02:
            print(f"✅ 恢复出厂设置成功（需断电重启并重新校准编码器）")
            return True
        else:
            print(f"❌ 恢复出厂设置失败")
            return False

    # -------------------------- 4. 参数读取功能（文档6.3.4节） --------------------------
    def read_version(self):
        """读取固件/硬件版本（文档6.3.4节）"""
        cmd = [FUNC_READ_VERSION, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 读取版本命令发送失败")
            return None
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 6:
            print(f"❌ 读取版本响应异常")
            return None
        
        firmware_ver = f"Emm42_V5.0.{resp_data[2]}"
        hardware_ver = f"ZDT_X42_V{resp_data[3]/10:.1f}"
        print(f"✅ 版本信息：固件={firmware_ver}，硬件={hardware_ver}")
        return (firmware_ver, hardware_ver)

    def read_bus_voltage(self):
        """读取总线电压（文档6.3.4节）"""
        cmd = [FUNC_READ_BUS_VOLT, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 读取总线电压命令发送失败")
            return None
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 5:
            print(f"❌ 读取总线电压响应异常")
            return None
        
        voltage_mv = (resp_data[2] << 8) | resp_data[1]
        print(f"✅ 总线电压：{voltage_mv/1000:.2f}V")
        return voltage_mv/1000

    def read_phase_current(self):
        """读取相电流（文档6.3.4节）"""
        cmd = [FUNC_READ_PHASE_CUR, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 读取相电流命令发送失败")
            return None
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 5:
            print(f"❌ 读取相电流响应异常")
            return None
        
        current_ma = (resp_data[3] << 8) | resp_data[2]
        print(f"✅ 相电流：{current_ma}mA")
        return current_ma

    def read_real_speed(self):
        """读取实时转速（文档6.3.4节）"""
        cmd = [FUNC_READ_REAL_SPEED, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 读取实时转速命令发送失败")
            return None
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 5:
            print(f"❌ 读取实时转速响应异常")
            return None
        
        speed_rpm = (resp_data[2] << 8) | resp_data[3]
        direction = "CCW" if resp_data[1] == 0x01 else "CW"
        #print(f"✅ 实时转速：{speed_rpm}RPM（方向：{direction}）")
        return (speed_rpm, direction)

    def read_real_position(self):
        
        """读取实时位置（文档6.3.4节）"""
        cmd = [FUNC_READ_REAL_POS, CHECK_SUM]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 读取实时位置命令发送失败")
            return None
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if not resp_ok or len(resp_data) != 7:
            print(f"❌ 读取实时位置响应异常")
            return None
        
        # 调试信息：显示原始数据
        print(f"🔍 实时位置原始数据: {resp_data}")
        
        # 正确解析位置值：第2-5字节为32位有符号整数（大端字节序）
        # 数据格式：[0x36, 0x01, 0x00, 0x08, 0xff, 0xfb, 0x6b]
        # 第2-5字节：0x01 0x00 0x08 0xff 0xfb
        # 实际位置值应该是：0x010008fffb（32位整数）
        
        # 将第2-5字节组合成32位整数（大端字节序：resp_data[2]是最高位，resp_data[5]是最低位）
        pos_value = (resp_data[2] << 24) | (resp_data[3] << 16) | (resp_data[4] << 8) | resp_data[5]
        
        # 转换为有符号32位整数
        if pos_value & 0x80000000:  # 检查第32位是否为1（负数）
            pos_value = pos_value - 0x100000000
        
        print(f"🔍 编码器原始值: {pos_value} (0x{pos_value & 0xFFFFFFFF:08X})")
        
        # 角度计算：编码器值 * 360 / 65536
        pos_angle = (pos_value * 360) / 65536
        
        # 方向判断：第1字节为方向标志
        direction = "负方向" if resp_data[1] == 0x01 else "正方向"
        
        print(f"✅ 实时位置：编码器值={pos_value}, 角度={pos_angle:.2f}°（{direction}）")
        return pos_angle

    # -------------------------- 5. 参数修改功能（文档6.3.5节） --------------------------
    def modify_microstep(self, microstep, save_flag=0x01):
        """修改细分（文档6.3.5节，1-256，0x00=256细分）"""
        if not (0x00 <= microstep <= 0xFF):
            print(f"❌ 细分值无效（需0-255，0=256细分）")
            return False
        
        cmd = [
            FUNC_MODIFY_MICROSTEP, 0x8A,
            save_flag, microstep, CHECK_SUM
        ]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 修改细分命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if resp_ok and resp_data[2] == 0x02:
            actual_step = 256 if microstep == 0x00 else microstep
            print(f"✅ 细分修改成功：{actual_step}细分（{'断电保存' if save_flag==0x01 else '临时生效'}）")
            return True
        else:
            print(f"❌ 细分修改失败")
            return False

    def modify_id_address(self, new_id, save_flag=0x01):
        """修改ID地址（文档6.3.5节，1-255）"""
        if not (0x01 <= new_id <= 0xFF):
            print(f"❌ ID地址无效（需1-255）")
            return False
        
        cmd = [
            FUNC_MODIFY_ID, 0x4B,
            save_flag, new_id, CHECK_SUM
        ]
        self.current_send_id = (self.device_id << 8) | 0x00
        send_ok = self._send_can_data(self.current_send_id, cmd, is_ext_frame=True)
        if not send_ok:
            print(f"❌ 修改ID命令发送失败")
            return False
        
        resp_ok, resp_data = self._receive_can_data(self.current_send_id, is_ext_frame=True)
        if resp_ok and resp_data[2] == 0x02:
            print(f"✅ ID地址修改成功：{new_id}（需断电重启生效，{'断电保存' if save_flag==0x01 else '临时生效'}）")
            self.device_id = new_id  # 更新当前ID
            return True
        else:
            print(f"❌ ID地址修改失败")
            return False

# -------------------------- 6. 功能测试菜单系统 --------------------------
def test_all_functions():
    """测试所有功能函数 - 交互式菜单系统"""
    
    # 1. 初始化CAN设备
    print("📡 初始化CAN设备...")
    m_dev, dev_ch0, dev_ch1 = init_can_device( 
        baud_rate=500000,        # 仲裁域波特率
        dbit_baud_rate=500000,  # 数据域波特率（CANFD模式）
        channels=[1],            # 初始化通道1
        can_type=TYPE_CANFD,     # 使用CANFD模式
        canfd_standard=0         # ISO标准
    )
    if not m_dev or not dev_ch1:
        print("❌ CAN设备初始化失败，程序退出")
        return
    
    channel_handle = dev_ch1  # 使用通道1的句柄
    
    try:
        # 2. 初始化Emm42控制器（ID=1）
        print("🔧 初始化Emm42控制器（ID=1）...")
        emm42 = Emm42_CAN_Controller(
            channel_handle=channel_handle,
            device_id=1,
            response_timeout=5
        )
        
        # 3. 交互式测试菜单
        while True:
            print("\n" + "="*60)
            print("🎯 EMM42电机控制器功能测试菜单")
            print("="*60)
            print("1.  电机使能/失能控制")
            print("2.  速度模式控制")
            print("3.  位置模式控制")
            print("4.  立即停止")
            print("5.  多机同步运动")
            print("6.  设置回零零点")
            print("7.  触发回零")
            print("8.  中断回零")
            print("9.  读取回零参数")
            print("10. 编码器校准")
            print("11. 位置清零")
            print("12. 解除堵转保护")
            print("13. 恢复出厂设置")
            print("14. 读取版本信息")
            print("15. 读取总线电压")
            print("16. 读取相电流")
            print("17. 读取实时转速")
            print("18. 读取实时位置")
            print("19. 修改细分")
            print("20. 修改ID地址")
            print("21. 完整功能测试（按顺序执行所有功能）")
            print("0.  退出测试")
            print("="*60)
            
            choice = input("请选择测试功能 (0-21): ").strip()
            
            if choice == "0":
                print("👋 退出测试程序")
                break
            
            elif choice == "1":
                print("\n🔌 电机使能/失能控制测试")
                enable = input("是否使能电机？(y/n): ").strip().lower() == 'y'
                emm42.motor_enable(enable=enable)
                
            elif choice == "2":
                print("\n⚡ 速度模式控制测试")
                direction = int(input("方向 (0=CW顺时针, 1=CCW逆时针): "))
                speed = int(input("转速 (RPM): "))
                accel = int(input("加速度档位 (0-255): "))
                emm42.speed_control(direction=direction, speed_rpm=speed, accel_level=accel)
                
            elif choice == "3":
                print("\n🎯 位置模式控制测试")
                direction = int(input("方向 (0=CW顺时针, 1=CCW逆时针): "))
                speed = int(input("转速 (RPM): "))
                accel = int(input("加速度档位 (0-255): "))
                pulses = int(input("脉冲数 (16细分下3200脉冲=1圈): "))
                mode = int(input("位置模式 (0=相对, 1=绝对): "))
                emm42.position_control(
                    direction=direction, 
                    speed_rpm=speed, 
                    accel_level=accel,
                    pulse_num=pulses, 
                    pos_mode=mode
                )
                
            elif choice == "4":
                print("\n🛑 立即停止测试")
                emm42.immediate_stop()
                
            elif choice == "5":
                print("\n🔄 多机同步运动测试")
                emm42.sync_motion()
                
            elif choice == "6":
                print("\n📍 设置回零零点测试")
                save = int(input("保存标志 (0=临时生效, 1=断电保存): "))
                emm42.set_homing_zero(save_flag=save)
                
            elif choice == "7":
                print("\n🏠 触发回零测试")
                mode = int(input("回零模式 (0=就近, 1=方向, 2=无传感器, 3=限位开关): "))
                emm42.trigger_homing(homing_mode=mode)
                
            elif choice == "8":
                print("\n⏹️ 中断回零测试")
                emm42.abort_homing()
                
            elif choice == "9":
                print("\n📊 读取回零参数测试")
                emm42.read_homing_param()
                
            elif choice == "10":
                print("\n🔧 编码器校准测试")
                emm42.calibrate_encoder()
                
            elif choice == "11":
                print("\n🔄 位置清零测试")
                emm42.clear_position()
                
            elif choice == "12":
                print("\n🔓 解除堵转保护测试")
                emm42.clear_lock_protection()
                
            elif choice == "13":
                print("\n🏭 恢复出厂设置测试")
                emm42.restore_factory()
                
            elif choice == "14":
                print("\n📄 读取版本信息测试")
                emm42.read_version()
                
            elif choice == "15":
                print("\n🔋 读取总线电压测试")
                emm42.read_bus_voltage()
                
            elif choice == "16":
                print("\n⚡ 读取相电流测试")
                emm42.read_phase_current()
                
            elif choice == "17":
                print("\n📈 读取实时转速测试")
                emm42.read_real_speed()
                
            elif choice == "18":
                print("\n🎯 读取实时位置测试")
                emm42.read_real_position()
                
            elif choice == "19":
                print("\n⚙️ 修改细分测试")
                step = int(input("细分值 (0-255, 0=256细分): "))
                save = int(input("保存标志 (0=临时生效, 1=断电保存): "))
                emm42.modify_microstep(microstep=step, save_flag=save)
                
            elif choice == "20":
                print("\n🆔 修改ID地址测试")
                new_id = int(input("新ID地址 (1-255): "))
                save = int(input("保存标志 (0=临时生效, 1=断电保存): "))
                emm42.modify_id_address(new_id=new_id, save_flag=save)
                
            elif choice == "21":
                print("\n🚀 开始完整功能测试（按顺序执行所有功能）")
                print("="*60)
                
                # 1. 基础控制
                print("\n🔌 1. 电机使能")
                emm42.motor_enable(enable=True)
                time.sleep(1)
                
                print("\n⚡ 2. 速度控制")
                emm42.speed_control(direction=0x01, speed_rpm=50, accel_level=0x01)
                time.sleep(3)
                
                print("\n🛑 3. 立即停止")
                emm42.immediate_stop()
                time.sleep(1)
                
                print("\n🎯 4. 位置控制")
                emm42.position_control(
                    direction=0x01, speed_rpm=50, accel_level=0x01,
                    pulse_num=3200, pos_mode=POS_MODE_RELATIVE
                )
                time.sleep(3)
                
                # 2. 回零功能
                print("\n📍 5. 设置回零零点")
                emm42.set_homing_zero(save_flag=0x01)
                time.sleep(1)
                
                print("\n🏠 6. 触发回零")
                emm42.trigger_homing(homing_mode=HOMING_MODE_NEAREST)
                time.sleep(3)
                
                print("\n⏹️ 7. 中断回零")
                emm42.abort_homing()
                time.sleep(1)
                
                # 3. 参数读取
                print("\n📊 8. 读取回零参数")
                emm42.read_homing_param()
                time.sleep(1)
                
                print("\n🔋 9. 读取总线电压")
                emm42.read_bus_voltage()
                time.sleep(1)
                
                print("\n⚡ 10. 读取相电流")
                emm42.read_phase_current()
                time.sleep(1)
                
                print("\n📈 11. 读取实时转速")
                emm42.read_real_speed()
                time.sleep(1)
                
                print("\n🎯 12. 读取实时位置")
                emm42.read_real_position()
                time.sleep(1)
                
                # 4. 参数修改
                print("\n⚙️ 13. 修改细分")
                emm42.modify_microstep(microstep=16, save_flag=0x01)
                time.sleep(1)
                
                # 5. 紧急操作
                print("\n🔄 14. 位置清零")
                emm42.clear_position()
                time.sleep(1)
                
                print("\n🔓 15. 解除堵转保护")
                emm42.clear_lock_protection()
                time.sleep(1)
                
                print("\n🏭 16. 恢复出厂设置")
                emm42.restore_factory()
                time.sleep(1)
                
                print("\n📄 17. 读取版本信息")
                emm42.read_version()
                time.sleep(1)
                
                print("\n🆔 18. 修改ID地址")
                emm42.modify_id_address(new_id=2, save_flag=0x01)
                time.sleep(1)
                
                print("\n🔧 19. 编码器校准")
                emm42.calibrate_encoder()
                time.sleep(1)
                
                print("\n🔄 20. 多机同步运动")
                emm42.sync_motion()
                time.sleep(1)
                
                print("\n✅ 完整功能测试完成！")
                
            else:
                print("❌ 无效选择，请重新输入")
                
            # 等待用户确认继续
            if choice != "0":
                input("\n按Enter键继续...")
                
    except Exception as e:
        print(f"\n❌ 程序运行异常：{str(e)}")
    finally:
        # 关闭CAN设备
        close_can_device(m_dev, dev_ch0, dev_ch1)
        print("✅ CAN设备已关闭")

# -------------------------- 7. 程序主函数 --------------------------
def main():
    """主函数 - 提供简单的测试流程"""
    
    # 1. 初始化CAN设备
    m_dev, dev_ch0, dev_ch1 = init_can_device( 
        baud_rate=500000,        # 仲裁域波特率
        dbit_baud_rate=500000,  # 数据域波特率（CANFD模式）
        channels=[1],            # 初始化通道1
        can_type=TYPE_CANFD,     # 使用CANFD模式
        canfd_standard=0         # ISO标准
    )
    if not m_dev or not dev_ch1:
        print("❌ CAN设备初始化失败，程序退出")
        return
    
    channel_handle = dev_ch1  # 使用通道1的句柄
    
    try:
        # 2. 初始化Emm42控制器（ID=1）
        emm42 = Emm42_CAN_Controller(
            channel_handle=channel_handle,
            device_id=1,
            response_timeout=5
        )

        # 3. 功能测试流程（按需求注释/启用）
        print("\n" + "="*50 + " 开始功能测试 " + "="*50)
        
        # 3.1 基础控制：使能电机→速度控制→立即停止
        emm42.motor_enable(enable=True)
        time.sleep(1)
        emm42.speed_control(direction=0x01, speed_rpm=50, accel_level=0x01)
        time.sleep(5)  # 运行5秒
        emm42.immediate_stop()
        time.sleep(1)

        # 3.2 位置控制：相对位置模式（16细分，3200脉冲=1圈）
        emm42.position_control(
            direction=0x01,
            speed_rpm=50,
            accel_level=0x01,
            pulse_num=32000,  # 1圈
            pos_mode=POS_MODE_RELATIVE
        )
        time.sleep(3)

        # # 3.3 原点回零：设置零点→触发单圈就近回零
        # emm42.set_homing_zero(save_flag=0x01)
        # time.sleep(1)
        # emm42.trigger_homing(homing_mode=HOMING_MODE_NEAREST)
        # time.sleep(5)

        # # 3.4 参数读取：总线电压→相电流→实时位置
        # emm42.read_bus_voltage()
        # emm42.read_phase_current()
        # emm42.read_real_position()

        # # 3.5 参数修改：细分改为32→读取验证（需重新读取驱动参数确认）
        # emm42.modify_microstep(microstep=32, save_flag=0x01)
        # time.sleep(1)

        # # 3.6 紧急操作：位置清零→解除堵转保护（模拟）
        # emm42.clear_position()
        # emm42.clear_lock_protection()

        print("\n" + "="*50 + " 功能测试结束 " + "="*50)

    except Exception as e:
        print(f"\n❌ 程序运行异常：{str(e)}")
    finally:
        # 4. 关闭CAN设备
        close_can_device(m_dev, dev_ch0, dev_ch1)

if __name__ == "__main__":
    print("🎯 EMM42电机控制器测试程序")
    print("功能菜单:")
    print("1. 交互式功能测试（推荐）")
    print("2. 简单功能测试")
    print("3. 仅初始化测试")
    print("4. 退出")
    
    choice = input("请选择测试模式 (1-4): ").strip()
    
    if choice == "1":
        test_all_functions()
    elif choice == "2":
        main()
    elif choice == "3":
        print("📡 仅初始化测试...")
        m_dev, dev_ch0, dev_ch1 = init_can_device( 
            baud_rate=500000,        # 仲裁域波特率
            dbit_baud_rate=500000,  # 数据域波特率（CANFD模式）
            channels=[1],            # 初始化通道1
            can_type=TYPE_CANFD,     # 使用CANFD模式
            canfd_standard=0         # ISO标准
        )
        if m_dev and dev_ch1:
            print("✅ CAN设备初始化成功")
            close_can_device(m_dev, dev_ch0, dev_ch1)
            print("✅ CAN设备已关闭")
        else:
            print("❌ CAN设备初始化失败")
    elif choice == "4":
        print("👋 退出测试程序")
    else:
        print("❌ 无效选择，退出程序")